package com.csjbot.snowbot_rogue.servers.slams;

import android.content.Context;
import android.graphics.Bitmap;
import android.graphics.BitmapFactory;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Matrix;
import android.graphics.Paint;
import android.graphics.PointF;
import android.graphics.RectF;
import android.util.Log;

import com.android.core.util.SharedUtil;
import com.csjbot.csjbase.log.Csjlogger;
import com.csjbot.snowbot_rogue.Events.TestDataEvent;
import com.csjbot.snowbot_rogue.R;
import com.csjbot.snowbot_rogue.bean.MapSize;
import com.csjbot.snowbot_rogue.servers.slams.agent.RPSlamwareSdpAgent;
import com.csjbot.snowbot_rogue.servers.slams.events.ConnectedEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.ConnectionLostEvent;
import com.csjbot.snowbot_rogue.servers.slams.events.RobotStatusUpdateEvent;
import com.csjbot.snowbot_rogue.utils.Constant;
import com.slamtec.slamware.SlamwareCorePlatform;
import com.slamtec.slamware.action.ActionStatus;
import com.slamtec.slamware.action.IAction;
import com.slamtec.slamware.action.IMoveAction;
import com.slamtec.slamware.action.MoveDirection;
import com.slamtec.slamware.geometry.Line;
import com.slamtec.slamware.robot.ArtifactUsage;
import com.slamtec.slamware.robot.Location;
import com.slamtec.slamware.robot.Map;
import com.slamtec.slamware.robot.MapKind;
import com.slamtec.slamware.robot.MapType;
import com.slamtec.slamware.robot.MoveOption;
import com.slamtec.slamware.robot.NetworkMode;
import com.slamtec.slamware.robot.Pose;

import org.greenrobot.eventbus.EventBus;
import org.greenrobot.eventbus.Subscribe;
import org.greenrobot.eventbus.ThreadMode;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Vector;

/**
 * Created by Administrator on 2016/8/4 0004.
 */
public class SnowBotMoveServer {
    private static MoveServerMapListener moveServerMapListener;
    private static SlamwareCorePlatform platform = null;
    private static SnowBotMoveServer ourInstance = new SnowBotMoveServer();
    private RPSlamwareSdpAgent rpSlamwareSdpAgent = new RPSlamwareSdpAgent();

    private static final String SLAMTEC_IP = Constant.SLAM_DEFAULT_IP;
    private static final int RECT_LEN = 18;
    private static final int SLAMTEC_PORT = 1445;
    private static float MAP_ROTATE_DEGREE = 5f;

    private EventBus eventBus;
    private List<Location> locations = new ArrayList<>();
    private Paint paint = new Paint();
    private Pose currentPose;
    private RectF rectF = new RectF(-RECT_LEN, -RECT_LEN, RECT_LEN, RECT_LEN);
    private Vector<Line> walls = new Vector<>();

    private boolean isGetting;
    private boolean isRunning = false;
    private boolean wakeupMapUpdate;
    private float resolutionX = 0F, resolutionY = 0F;
    private int showMapW, showMapH;
    private int slamMapW = 0, slamMapH = 0;
    private boolean isCircleRunning = false;

    public void setIsInRecoveryMapData(boolean recoveryMapData) {
        isRecoveryMapData = recoveryMapData;
    }

    public int getShowMapH() {
        return showMapH;
    }

    private boolean isRecoveryMapData;

    public void setWakeupMapUpdate(boolean wakeupMapUpdate) {
        this.wakeupMapUpdate = wakeupMapUpdate;
    }

    public static SnowBotMoveServer getInstance() {
        return ourInstance;
    }

    public void setMoveServerMapListener(MoveServerMapListener listener) {
        moveServerMapListener = listener;
    }

    private SnowBotMoveServer() {
        Csjlogger.info("==== {} started ", this.getClass().getSimpleName());
        paint.setColor(Color.RED);
        eventBus = EventBus.getDefault();
        eventBus.register(this);

        connect();
    }

    public void connect() {
        Csjlogger.debug("SnowBotMoveServer begin connect");
        rpSlamwareSdpAgent.connectTo(SLAMTEC_IP, SLAMTEC_PORT);
    }

    /**
     * 转角度
     *
     * @param degree 度数
     */
    public void turnRound(final short degree) {
        rpSlamwareSdpAgent.rotate(degree);
    }

    public void cancelAction() {
        rpSlamwareSdpAgent.cancelAllActions();
    }

    public void turnRound(final MoveDirection direction, final int count) {
        rpSlamwareSdpAgent.turnRound(direction, count);
    }

    public void moveBy(final MoveDirection direction) {
        rpSlamwareSdpAgent.moveBy(direction);
    }

    public void moveTo(Location location) {
        rpSlamwareSdpAgent.moveTo(location);
    }


    private void reColor(int[] data) {
        for (int i = 0; i < data.length; ++i) {
//            Log.d(this.getClass().getSimpleName(), "map data is :" + data[i]);
            if (data[i] == 0) {
                data[i] = 0xaaaaaa;
//            } else if (data[i] == -127 ) { // 是墙体
            } else if (data[i] < 0) { // 是墙体
                data[i] = 0x000000;
            } else {
                data[i] = 0xFFFFFF;
            }
        }
    }

    private Matrix matrixMap = new Matrix(), matrixOriMap = new Matrix(), matrixArrow = new Matrix();

    public Bitmap superposition(Bitmap oriMap, Bitmap arrow, float angel, int offsetX, int offsetY) {
        oriMap = Bitmap.createBitmap(oriMap, 0, 0, oriMap.getWidth(), oriMap.getHeight(), matrixOriMap, true);

        // 另外创建一张图片
        // 创建一个新的和SRC长度宽度一样的位图
        Bitmap newb = Bitmap.createBitmap(oriMap.getWidth(), oriMap.getHeight(), Bitmap.Config.RGB_565);
        Canvas canvas = new Canvas(newb);

        canvas.drawBitmap(oriMap, 0, 0, null);// 在 0，0坐标开始画入原图片src


        int oriMapW = oriMap.getWidth(), oriMapH = oriMap.getHeight();
        //旋转图片 动作
        matrixArrow.postRotate((MAP_ROTATE_DEGREE - angel + 90f)); // 角度修正 90f为图片的原始角度
        arrow = Bitmap.createBitmap(arrow, 0, 0, arrow.getWidth(), arrow.getHeight(), matrixArrow, true); // 旋转图片
        canvas.drawBitmap(arrow, (oriMapH - arrow.getHeight()) / 2 - offsetY, (oriMapW - arrow.getWidth()) / 2 - offsetX, null);

        canvas.save(Canvas.ALL_SAVE_FLAG);
        canvas.restore();

        for (Line wall : walls) {
            canvas.drawLine(oriMapH / 2 - wall.getStartY() / 0.05f, oriMapW / 2 - wall.getStartX() / 0.05f,
                    oriMapH / 2 - wall.getEndY() / 0.05f, oriMapW / 2 - wall.getEndX() / 0.05f, paint);
        }

        arrow.recycle();
        matrixArrow.reset();
        newb = Bitmap.createBitmap(newb, 0, 0, newb.getWidth(), newb.getHeight(), matrixMap, true);

        return newb;
    }

    public Bitmap superpositionNonArrow(Bitmap oriMap, Bitmap arrow, float angel, int offsetX, int offsetY) {
        oriMap = Bitmap.createBitmap(oriMap, 0, 0, oriMap.getWidth(), oriMap.getHeight(), matrixOriMap, true);

        // 另外创建一张图片
        // 创建一个新的和SRC长度宽度一样的位图
        Bitmap newb = Bitmap.createBitmap(oriMap.getWidth(), oriMap.getHeight(), Bitmap.Config.RGB_565);
        Canvas canvas = new Canvas(newb);

        canvas.drawBitmap(oriMap, 0, 0, null);// 在 0，0坐标开始画入原图片src


        int oriMapW = oriMap.getWidth(), oriMapH = oriMap.getHeight();
        //旋转图片 动作
        matrixArrow.postRotate((MAP_ROTATE_DEGREE - angel + 90f)); // 角度修正 90f为图片的原始角度
        arrow = Bitmap.createBitmap(arrow, 0, 0, arrow.getWidth(), arrow.getHeight(), matrixArrow, true); // 旋转图片
        canvas.save(Canvas.ALL_SAVE_FLAG);
        canvas.restore();

        for (Line wall : walls) {
            canvas.drawLine(oriMapH / 2 - wall.getStartY() / 0.05f, oriMapW / 2 - wall.getStartX() / 0.05f,
                    oriMapH / 2 - wall.getEndY() / 0.05f, oriMapW / 2 - wall.getEndX() / 0.05f, paint);
        }
        arrow.recycle();
        matrixArrow.reset();
        newb = Bitmap.createBitmap(newb, 0, 0, newb.getWidth(), newb.getHeight(), matrixMap, true);

        return newb;
    }


    public void stopGetMap() {
        moveServerMapListener = null;
        isGetting = false;
    }

    public Bitmap getBitMap(Context context) {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return null;
        }

        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

        Bitmap finalMap = null;
        if (platform != null) {
            Map map = platform.getMap(MapType.BITMAP_8BIT, rectF); // 获取地图数据
            currentPose = platform.getPose(); // 获取当前姿势
            walls = platform.getWalls();
            int[] newData = new int[map.getData().length]; // 获取地图内容
            int width = map.getDimension().getWidth();
            int height = map.getDimension().getHeight();

            int pos = 0;
            byte[] oldDate = map.getData(); // 获取原始数据

            showMapW = (int) ((RECT_LEN * 2) / map.getResolution().getX());
            showMapH = (int) ((RECT_LEN * 2) / map.getResolution().getY());
            // 第一步，坐标系转换，从右手坐标系转为左手坐标系
            // 机器坐标系和显示屏坐标系关于副对角线对称 故而从右下角开始取值
            for (int i = width - 1; i >= 0; --i) {
                for (int j = height - 1; j >= 0; --j) {
                    newData[pos++] = (int) oldDate[i + j * width];
                }
            }

            reColor(newData); // 重新着色
            double angle = 180 * currentPose.getYaw() / Math.PI; // 弧度->角度
            Bitmap bm = Bitmap.createBitmap(newData, height, width, Bitmap.Config.RGB_565); // 获取原始地图
            finalMap = superpositionNonArrow(bm, BitmapFactory.decodeResource(context.getResources(), R.mipmap.arrow), (float) angle,
                    (int) (currentPose.getX() / map.getResolution().getX()),
                    (int) (currentPose.getY() / map.getResolution().getY()));
        }

        return finalMap;
    }

    public void getMapSize() {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return;
        }

        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
        MapSize mapSize = new MapSize();

        if (platform != null) {
            if (slamMapW == 0 && slamMapH == 0) {
                Map map = platform.getMap(MapType.BITMAP_8BIT, rectF); // 获取地图数据

                slamMapW = map.getDimension().getWidth();
                slamMapH = map.getDimension().getHeight();

                resolutionX = map.getResolution().getX();
                resolutionY = map.getResolution().getY();
            }
            currentPose = platform.getPose(); // 获取当前姿势
            walls = platform.getWalls();

            double angle = 180 * currentPose.getYaw() / Math.PI; // 弧度->角度
            mapSize.setOffsetX(currentPose.getX() / resolutionX);
            mapSize.setOffsetY(currentPose.getY() / resolutionY);
            mapSize.setMapWidth(slamMapW);
            mapSize.setMapHeight(slamMapH);
            mapSize.setPostRotate((MAP_ROTATE_DEGREE - angle + 90f));
            SharedUtil.setObj("MAPSIZE", mapSize);
        }
    }


    public byte[] getMapOnce() {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return null;
        }

        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

        return platform.getMap(MapType.BITMAP_8BIT, rectF).getData();
    }

    private Map cachedMap = null;

    public void fakeWakeup() {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return;
        }
        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
        rpSlamwareSdpAgent.fakeWakeup();
    }


    public void getMap(final Context context, final MoveServerMapListener listener) {
        new Thread(new Runnable() {
            @Override
            public void run() {
                if (!isGetting) {
                    isGetting = true;

                    if (!rpSlamwareSdpAgent.isConnected()) {
                        return;
                    }

                    platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
                    rpSlamwareSdpAgent.fakeWakeup();
                    int cnt = 0;

                    try {
                        while (isGetting) {
                            if (cnt % 60 == 0) {
                                if (!isRecoveryMapData) {
                                    if (!isCircleRunning) {
                                        cachedMap = platform.getMap(MapType.BITMAP_8BIT, rectF); // 获取地图数据
                                    }
//                                    Csjlogger.warn("update Map");
                                    currentPose = platform.getPose(); // 获取当前姿势
                                    walls = platform.getWalls();

                                    int[] newData = new int[cachedMap.getData().length]; // 获取地图内容
                                    slamMapW = cachedMap.getDimension().getWidth();
                                    slamMapH = cachedMap.getDimension().getHeight();

                                    int pos = 0;
                                    byte[] oldDate = cachedMap.getData(); // 获取原始数据

                                    showMapW = (int) ((RECT_LEN * 2) / cachedMap.getResolution().getX());
                                    showMapH = (int) ((RECT_LEN * 2) / cachedMap.getResolution().getY());

                                    // 第一步，坐标系转换，从右手坐标系转为左手坐标系
                                    // 机器坐标系和显示屏坐标系关于副对角线对称 故而从右下角开始取值
                                    for (int i = slamMapW - 1; i >= 0; --i) {
                                        for (int j = slamMapH - 1; j >= 0; --j) {
                                            newData[pos++] = (int) oldDate[i + j * slamMapW];
                                        }
                                    }

                                    Bitmap finalMap;

                                    reColor(newData); // 重新着色
                                    double angle = 180 * currentPose.getYaw() / Math.PI; // 弧度->角度
                                    Bitmap bm = Bitmap.createBitmap(newData, slamMapH, slamMapW, Bitmap.Config.RGB_565); // 获取原始地图

                                    finalMap = superposition(bm, BitmapFactory.decodeResource(context.getApplicationContext().getResources(), R.mipmap.arrow), (float) angle,
                                            (int) (currentPose.getX() / cachedMap.getResolution().getX()),
                                            (int) (currentPose.getY() / cachedMap.getResolution().getY()));
                                    if (listener != null) {
                                        listener.getMap(finalMap);
                                    }
                                }

                                System.gc();
                            }

                            cnt++;

                            if (cnt == Integer.MAX_VALUE) {
                                cnt = 0;
                            }

                            try {
                                Thread.sleep(33);
                            } catch (InterruptedException e) {
                                e.printStackTrace();
                            }
                        }

                    } catch (java.lang.Exception e) {
//                        rpSlamwareSdpAgent.reconnect();
                        Csjlogger.error("Exception", e);
                    }
                }
            }
        }).start();
    }

    /**
     * 获取地图数据，用于保存地图数据
     *
     * @return
     */
    public Map getBackUpMap() {
        if (null != platform) {
            return platform.getMap(MapType.BITMAP_8BIT, rectF);
        }

        return null;
    }

    /**
     * 用于保存虚拟墙数据
     *
     * @return
     */
    public List<Line> getBackUpWalls() {
        if (null != platform) {
            return platform.getLines(ArtifactUsage.ArtifactUsageVirutalWall);
        }

        return null;
    }

    /**
     * 还原数据
     */
    public void recoveryMapData(Map map, List<Line> list) {
        if (rpSlamwareSdpAgent.isConnected() && null != platform) {
            isRecoveryMapData = true;
            platform.setMap(map, MapType.BITMAP_8BIT, MapKind.EXPLORE_MAP);
            platform.addLines(ArtifactUsage.ArtifactUsageVirutalWall, list);
        }
    }

    public Pose getCurrentPose() {
        return currentPose;
    }

    public Pose getPose() {
        return rpSlamwareSdpAgent.getRobotPose();
    }

    public void stopPartol() {
        isCircleRunning = false;
        if (rpSlamwareSdpAgent.isConnected() && platform != null) {
            IMoveAction moveAction = platform.getCurrentAction();
            if (moveAction != null) {
                moveAction.cancel();
            }
        }
    }

    private boolean isCircleRunning() {
        return isCircleRunning;
    }

    public void setAutoUpdateMap(boolean autoUpdateMap) {
//        rpSlamwareSdpAgent.setAutoUpdateMap(autoUpdateMap);
        if (rpSlamwareSdpAgent.isConnected() && platform != null) {
//            Csjlogger.debug("setAutoUpdateMap {} ", autoUpdateMap);
            platform.setMapUpdate(autoUpdateMap);
        }
    }

    private int errorCount;

    public int getErrorCount(){
        return errorCount;
    }

    public void partol(List<Pose> poseList) {
        errorCount = 0;
        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
        if (rpSlamwareSdpAgent.isConnected() && platform != null) {
            locations.clear();

            for (Pose pose : poseList) {
                locations.add(pose.getLocation());
            }

            isCircleRunning = true;

            new Thread(new Runnable() {
                @Override
                public void run() {
                    IMoveAction moveAction = platform.getCurrentAction();
                    if (moveAction != null) {
                        moveAction.cancel();
                    }


                    // 设置参数 MoveOption
                    MoveOption opt = new MoveOption();
                    opt.setAppending(true);
                    opt.setMilestone(true);

//                if (circleRunWithVt_) {
//                    opt.setKeyPoints(true);
//                }
                    // 初始化判断变量
                    int index = 0;
                    int max = locations.size();
                    int lastPointSize = 0;
                    // 第一个点
                    Location location = locations.get(index);

                    IMoveAction action = platform.moveTo(location, opt);
                    Csjlogger.debug("start circle run");
                    Csjlogger.debug("isCircleRunning {}", isCircleRunning);
                    while (isCircleRunning) {
                        if (action != null) {
                            // 获取执行的状态
                            ActionStatus actionStatus = action.waitUntilDone();
                            // 判断是否执行完毕
                            if (actionStatus == ActionStatus.ERROR) {
                                errorCount++;
                            }
                            // 取剩余的点
                            int leftPointSize = action.getRemainingPath().getPoints().size();
                            int remainingMilestones = action.getRemainingMilestones().getPoints().size();

                            // 先判断任务是否被取消
                            Csjlogger.debug("leftPointSize is {} lastPointSize is {} , lastPointSize - leftPointSize  = {} ",
                                    leftPointSize, lastPointSize, lastPointSize - leftPointSize);

                            if (leftPointSize > 0 && (lastPointSize - leftPointSize > 2)) {
                                // 理论上说 lastPointSize - leftPointSize 应该等于1（除了第一次）
                                // 但是有特殊情况，
                                // 如果大于2，则说明路径突然没有了，任务被取消了
                                // 继续走上一个点
                                action = platform.moveTo(location, opt);
                                Csjlogger.info("action cancled, continue goning to point");
                                // TODO 打印
                            } else {
                                if (remainingMilestones < 3) {
                                    lastPointSize = leftPointSize;
                                    // 剩余的点是否 <= 1，如果小于等于 1， 则需要走下面一个点了
                                    if (leftPointSize <= 1) {
                                        index++;
                                        if (index >= max) {
                                            index = 0;
                                        }

                                        location = locations.get(index);
                                        action = platform.moveTo(location, opt);
                                        Csjlogger.info("Go to next location, index = {}", index);
                                    } else {
//                            EventBus.getDefault().post(new AutoRunPath(leftPointSize));
                                        //TODO: 2017/3/30 打印信息
//                                Csjlogger.debug("剩余路径点 " + leftPointSize);
                                        Csjlogger.warn("剩余路径点 {}", leftPointSize);
                                    }
                                }
                            }
                        } else {
//                    Csjlogger.error("action == null ");
                            isCircleRunning = false;
                            break;
                            //TODO 报错
                        }

                        try {
                            Thread.sleep(300);
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        }
                    }
                }
            }).start();
        }
//        rpSlamwareSdpAgent.circleRun(locations, false);
    }



    private int partolCount = 0;
    public void partol(List<Pose> poseList,final int count) {
        errorCount = 0;
        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
        if (rpSlamwareSdpAgent.isConnected() && platform != null) {
            locations.clear();

            for (Pose pose : poseList) {
                locations.add(pose.getLocation());
            }

            new Thread(new Runnable() {
                @Override
                public void run() {
                    IMoveAction moveAction = platform.getCurrentAction();
                    if (moveAction != null) {
                        moveAction.cancel();
                    }


                    // 设置参数 MoveOption
                    MoveOption opt = new MoveOption();
                    opt.setAppending(true);
                    opt.setMilestone(true);

                    // 初始化判断变量
                    int index = 0;
                    int max = locations.size();
                    int lastPointSize = 0;
                    // 第一个点
                    Location location = locations.get(index);

                    IMoveAction action = platform.moveTo(location, opt);
                    Csjlogger.debug("start circle run");
                    Csjlogger.debug("isCircleRunning {}", isCircleRunning);
                    while (partolCount < (count*4)) {
                        if (action != null) {
                            // 获取执行的状态
                            ActionStatus actionStatus = action.waitUntilDone();
                            // 判断是否执行完毕
                            if (actionStatus == ActionStatus.ERROR) {
                                errorCount++;
                            }else if(actionStatus == ActionStatus.FINISHED){
                                partolCount++;
                            }
                            // 取剩余的点
                            int leftPointSize = action.getRemainingPath().getPoints().size();
                            int remainingMilestones = action.getRemainingMilestones().getPoints().size();

                            // 先判断任务是否被取消
                            Csjlogger.debug("leftPointSize is {} lastPointSize is {} , lastPointSize - leftPointSize  = {} ",
                                    leftPointSize, lastPointSize, lastPointSize - leftPointSize);

                            if (leftPointSize > 0 && (lastPointSize - leftPointSize > 2)) {
                                // 理论上说 lastPointSize - leftPointSize 应该等于1（除了第一次）
                                // 但是有特殊情况，
                                // 如果大于2，则说明路径突然没有了，任务被取消了
                                // 继续走上一个点
                                action = platform.moveTo(location, opt);
                                Csjlogger.info("action cancled, continue goning to point");
                                // TODO 打印
                            } else {
                                if (remainingMilestones < 3) {
                                    lastPointSize = leftPointSize;
                                    // 剩余的点是否 <= 1，如果小于等于 1， 则需要走下面一个点了
                                    if (leftPointSize <= 1) {
                                        index++;
                                        if (index >= max) {
                                            index = 0;
                                        }

                                        location = locations.get(index);
                                        action = platform.moveTo(location, opt);
                                        Csjlogger.info("Go to next location, index = {}", index);
                                    } else {
//                            EventBus.getDefault().post(new AutoRunPath(leftPointSize));
                                        //TODO: 2017/3/30 打印信息
//                                Csjlogger.debug("剩余路径点 " + leftPointSize);
                                        Csjlogger.warn("剩余路径点 {}", leftPointSize);
                                    }
                                }
                            }
                        } else {
                            break;
                            //TODO 报错
                        }

                        try {
                            Thread.sleep(300);
                        } catch (InterruptedException e) {
                            e.printStackTrace();
                        }
                    }
                }
            }).start();
        }
//        rpSlamwareSdpAgent.circleRun(locations, false);
    }

    public void addWall(Line wall) {
        if (platform != null) {
            platform.addWall(wall);
        }
    }

    public void stopCircleRun() {
        rpSlamwareSdpAgent.stopCircleRun();
    }

    public void clearWalls() {
        if (platform != null) {
            platform.clearWalls();
        }
    }

    public void moveTo(final float offsetX, final float offsetY) {
        new Thread(new Runnable() {
            @Override
            public void run() {
                Csjlogger.debug("moveTo " + offsetX + " " + offsetY);
                moveTo(new Location((-offsetY + showMapH / 2) * 0.05f,
                        (-offsetX + showMapW / 2) * 0.05f, 0f));
            }
        }).start();
    }


    public void goHome() {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return;
        }

        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

        new Thread(new Runnable() {
            @Override
            public void run() {
                IAction goHomeAction = platform.goHome();

                while (goHomeAction.getStatus() == ActionStatus.RUNNING) {

                    if (platform.getBatteryIsCharging()) {
                        break;
                    }

                    RectF rectF = new RectF(1, 1, -1, -1);
                    if (rectF.contains(platform.getPose().getX(), platform.getPose().getY())) {
                        goHomeAction = platform.goHome();
                    }

                    try {
                        Thread.sleep(200000);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }
                }


            }
        }).start();

//        rpSlamwareSdpAgent.goHome();
    }

    public void clearMap() {
//        rpSlamwareSdpAgent.fakeWakeup();
        rpSlamwareSdpAgent.clearMap();
    }

    public void addWall(PointF startP, PointF endP) {
        Line wall = new Line(walls.size() + 1, (-startP.y + showMapH / 2) * 0.05f, (-startP.x + showMapW / 2) * 0.05f,
                (-endP.y + showMapH / 2) * 0.05f, (-endP.x + showMapW / 2) * 0.05f);

        addWall(wall);
    }

    public void stopAllAction() {
        rpSlamwareSdpAgent.cancelAllActions();
    }

    public boolean connectWifi(String ssid, String passwd) {
        try {
            if (!rpSlamwareSdpAgent.isConnected()) {
                return false;
            }

            platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

            HashMap<String, String> opt = new HashMap<>();
            opt.put("ssid", ssid);
            opt.put("password", passwd);
            return platform.configureNetwork(NetworkMode.NetworkModeStation, opt);
        } catch (Exception e) {
            Csjlogger.error(e.getMessage());
            return false;
        }

    }

    public HashMap<String, String> getNetworkStatus() {
        try {
            if (!rpSlamwareSdpAgent.isConnected()) {
                return null;
            }

            platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

            return platform.getNetworkStatus();
        } catch (Exception e) {
            return null;
        }
    }

    public void destroy() {
        if (eventBus != null) {
            eventBus.unregister(this);
        }

        isRunning = false;
    }

    public boolean isSlamConnected() {
        return rpSlamwareSdpAgent.isConnected();
    }

    public boolean connectWifiNew() {
        if (!rpSlamwareSdpAgent.isConnected()) {
            return false;
        }

        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();

        HashMap<String, String> options = new HashMap<>();

        boolean ret;
//        ret = platform.configureNetwork(NetworkMode.NetworkModeWifiDisabled, options);
//        Csjlogger.info("config slam wifi set NetworkModeWifiDisabled is {}", ret);

        ret = platform.configureNetwork(NetworkMode.NetworkModeDHCPDisabled, options);
        Csjlogger.info("config slam wifi set NetworkModeDHCPDisabled is {}", ret);

//        options.put("ip", "192.168.99.2");
//        options.put("mask", "255.255.255.0");
//        options.put("gateway", "192.168.99.2");
//
//        ret = platform.configureNetwork(NetworkMode.NetworkModeWifiDisabled, options);
//        Csjlogger.info("config slam wifi set ip addr is {} ,options is {}", ret, options.toString());

        return ret;
    }

    private class GetStatusRunnable implements Runnable {
        private int cnt;

        @Override
        public void run() {
            Csjlogger.info("start {} ", getClass().getSimpleName());
            while (isRunning) {
                if (cnt % 120 == 0) {
//                    rpSlamwareSdpAgent.updatePose();
                    if (rpSlamwareSdpAgent.isConnected()) {
                        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
                        currentPose = platform.getPose();
                    }
//                        agent.updateMoveAction();
                }

                if ((cnt % 120) == 0) {
                    if (!testPower) {
                        if (rpSlamwareSdpAgent.isConnected()) {
                            platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
                        }

//                        rpSlamwareSdpAgent.updateRobotStatus();
                        int batteryPercentage;
                        boolean isCharging;
                        int localizationQuality;

                        try {
                            batteryPercentage = platform.getBatteryPercentage();
                            isCharging = platform.getBatteryIsCharging();
//                            localizationQuality = shouldReadLocalizationQuliaty ? platform.getLocalizationQuality() : -1;
//                getRobotHealth();
                        } catch (Exception e) {
                            return;
                        }

                        if (batteryPercentage > 100) {
                            batteryPercentage = 100;
                        }

                        EventBus.getDefault().post(new RobotStatusUpdateEvent(batteryPercentage, isCharging,
                                platform.getLocalizationQuality()));
                        System.gc();
                    }
                }

                cnt++;

                try {
                    Thread.sleep(33);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }

            }
        }
    }

    private boolean testPower = false;

    @Subscribe(threadMode = ThreadMode.POSTING)
    public void testDataSwitch(TestDataEvent event) {
        if (event.getTag() == 0) {
            testPower = (boolean) event.getObject();
        }
    }

    @Subscribe(threadMode = ThreadMode.POSTING)
    public void connectSlamSuccess(ConnectedEvent event) {
        Csjlogger.info("ConnectedEvent " + event.getMsg());
        new Thread(new GetStatusRunnable()).start();
        isRunning = true;
        platform = rpSlamwareSdpAgent.getRobotSlamPlatform();
    }

    @Subscribe(threadMode = ThreadMode.POSTING)
    public void connectSlamFailed(ConnectionLostEvent event) {
        Csjlogger.error("connectSlamFailed will restart in 5s");
        isRunning = false;

        try {
            Thread.sleep(5000);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }

        connect();
    }
}
